main_freertos_agm02_power_cycling.c File Reference

Include dependency graph for main_freertos_agm02_power_cycling.c:

Go to the source code of this file.

Functions
void	vApplicationIdleHook (void)
void	vApplicationTickHook (void)
static void	read_task (void *pvParameters)
static void	fusion_task (void *pvParameters)
bool	motionCheck (float sample[3], float baseline[3], float tolerance, uint32_t winLength, uint32_t *count)
int	main (void)

Variables
SensorFusionGlobals	sfg
ControlSubsystem	controlSubsystem
StatusSubsystem	statusSubsystem
PhysicalSensor	sensors [3]
EventGroupHandle_t	event_group = NULL

Detailed Description

This file shows one recommended way to incorporate sensor fusion capabilities into a FreeRTOS project AND adds intelligent power down of sensors when the DUT is stationary.

Definition in file main_freertos_agm02_power_cycling.c.

Function Documentation

static void fusion_task ( void *  pvParameters )

static

Definition at line 135 of file main_freertos_agm02_power_cycling.c.

Referenced by main(), and vApplicationTickHook().

{
    uint16_t i=0;  // general counter variable
    float motion_baseline[3] = {0.0, 0.0, 0.0};
    bool stationary;
    static bool lastStationary;
    uint32_t stationaryCount = 0;
    while (1)
    {
        xEventGroupWaitBits(event_group,    /* The event group handle. */
                            B0,             /* The bit pattern the event group is waiting for. */
                            pdTRUE,         /* BIT_0 and BIT_4 will be cleared automatically. */
                            pdFALSE,        /* Don't wait for both bits, either bit unblock task. */
                            portMAX_DELAY); /* Block indefinitely to wait for the condition to be met. */

        // sfg.runFusion(&sfg);             // Run the actual fusion algorithms
        // Rather than call runFusion directly, this example invokes the lower level
        // calls here.  This allows us to check to see if the board is stationary,
        // and if so, transition to a lower power state.
        sfg.conditionSensorReadings(&sfg);  // magCal is run as part of this
        stationary = motionCheck(
                    sfg.Accel.fGc,      // calibrated accelerometer reading
                    motion_baseline,    // baseline to check new values against
                    0.01,               // changes less than this are ignored
                    120,                // three seconds at 40Hz rate
                    &stationaryCount);
        if (stationary) {
            if (!lastStationary) {  // suspend some operations
                FXAS21002_Idle(&(sensors[1]), &sfg);
                MAG3110_Idle(&(sensors[2]), &sfg);
            }
            sfg.clearFIFOs(&sfg);
        } else {
            if (lastStationary) {   // restart operations
                FXAS21002_Init(&(sensors[1]), &sfg);
                MAG3110_Init(&(sensors[2]), &sfg);
            }
            // fuse the sensor data
            sfg.runFusion(&sfg);
        }

        sfg.applyPerturbation(&sfg);        // apply debug perturbation (testing only)

        sfg.loopcounter++;                  // The loop counter is used to "serialize" mag cal operations
        i=i+1;
        if (i>=4) {                         // Some status codes include a "blink" feature.  This loop
                i=0;                        // should cycle at least four times for that to operate correctly.
                sfg.updateStatus(&sfg);     // This is where pending status updates are made visible
        }
        if (stationary) {
            sfg.queueStatus(&sfg, LOWPOWER);    // assume LOWPOWER status for next pass through the loop
        } else {
            sfg.queueStatus(&sfg, NORMAL);      // assume NORMAL status for next pass through the loop
        }
        sfg.pControlSubsystem->stream(&sfg, sUARTOutputBuffer);      // Send stream data to the Sensor Fusion Toolbox
        lastStationary = stationary;
    }
}

Here is the call graph for this function:

Here is the caller graph for this function:

int main

(

void

)

This is a FreeRTOS (dual task) implementation of the NXP sensor fusion demo build.

Definition at line 90 of file main_freertos_agm02_power_cycling.c.

{
    ARM_DRIVER_I2C* I2Cdrv = &I2C_S_DRIVER_BLOCKING;       // defined in the <shield>.h file
    BOARD_InitPins();                   // defined in pin_mux.c, initializes pkg pins
    BOARD_BootClockRUN();               // defined in clock_config.c, initializes clocks
    BOARD_InitDebugConsole();           // defined in board.c, initializes the OpenSDA port

    I2Cdrv->Initialize(NULL);                                 // Initialize the KSDK driver for the I2C port
    I2Cdrv->Control(ARM_I2C_BUS_SPEED, ARM_I2C_BUS_SPEED_FAST);      // Configure the I2C bus speed

    initializeControlPort(&controlSubsystem);                           // configure pins and ports for the control sub-system
    initializeStatusSubsystem(&statusSubsystem);                        // configure pins and ports for the status sub-system
    initSensorFusionGlobals(&sfg, &statusSubsystem, &controlSubsystem); // Initialize sensor fusion structures
    // "install" the sensors we will be using
    sfg.installSensor(&sfg, &sensors[0], FXLS8952C_I2C_ADDR, 1, (void*) I2Cdrv, FXLS8952_Init,  FXLS8952_Read);
    sfg.installSensor(&sfg, &sensors[1], FXAS21002_I2C_ADDR, 1, (void*) I2Cdrv, FXAS21002_Init, FXAS21002_Read);
    sfg.installSensor(&sfg, &sensors[2], MAG3110_I2C_ADDR,   1, (void*) I2Cdrv, MAG3110_Init, MAG3110_Read);
    sfg.initializeFusionEngine(&sfg);           // This will initialize sensors and magnetic calibration

    event_group = xEventGroupCreate();
    xTaskCreate(read_task, "READ", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 2, NULL);
    xTaskCreate(fusion_task, "FUSION", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 1, NULL);

    sfg.setStatus(&sfg, NORMAL);                // If we got this far, let's set status state to NORMAL
    vTaskStartScheduler();                      // Start the RTOS scheduler
    sfg.setStatus(&sfg, HARD_FAULT);            // If we got this far, FreeRTOS does not have enough memory allocated
    for (;;) ;
}

Here is the call graph for this function:

bool motionCheck	(	float	sample[3],
		float	baseline[3],
		float	tolerance,
		uint32_t	winLength,
		uint32_t *	count
	)

The motionCheck() function is not a sensor fusion function. It is a function that simply monitors an accelerometer or magnetometer tri-axial sensor output, returning Boolean true if the sensor appears to be stationary, and false otherwise. This function would normally be called from your fusion_tasks in your main().

Parameters

sample	processed triaxial sensor sample (accel or mag)
baseline	previous value to compare to
tolerance	how much tolerance you can stand
winLength	how many samples need to be stable to assert "noMotion"
count	how many samples so far we've been not moving

Definition at line 44 of file motionCheck.c.

Referenced by fusion_task(), and vApplicationTickHook().

{
    float change[3];
    bool changed;
    change[CHX] = fabs(baseline[CHX] - sample[CHX]);
    change[CHY] = fabs(baseline[CHY] - sample[CHY]);
    change[CHZ] = fabs(baseline[CHZ] - sample[CHZ]);
    changed = (change[CHX]>tolerance) ||
              (change[CHY]>tolerance) ||
              (change[CHZ]>tolerance);
    if (changed) {
        baseline[CHX] = sample[CHX];
        baseline[CHY] = sample[CHY];
        baseline[CHZ] = sample[CHZ];
        *count = 0;
    } else {
        if ((*count) <= winLength) (*count) += 1;
    }
    return(*count > winLength);
}

static void read_task ( void *  pvParameters )

static

Definition at line 119 of file main_freertos_agm02_power_cycling.c.

Referenced by main(), and vApplicationTickHook().

{
    uint16_t i=0;                       // general counter variable
    portTickType lastWakeTime;
    const portTickType frequency = 1;   // tick counter runs at the read rate
    lastWakeTime = xTaskGetTickCount();
    while (1)
    {
        for (i=1; i<=OVERSAMPLE_RATE; i++) {
            vTaskDelayUntil(&lastWakeTime, frequency);
            sfg.readSensors(&sfg, i);              // Reads sensors, applies HAL and does averaging (if applicable)
        }
        xEventGroupSetBits(event_group, B0);
    }
}

Here is the caller graph for this function:

void vApplicationIdleHook

(

void

)

Definition at line 69 of file main_freertos_agm02_power_cycling.c.

{
  // MCU low power modes are not debugged at this point in time
    //volatile smc_power_state_t state;
    //SMC_SetPowerModeStop(SMC, kSMC_PartialStop );
    //state = SMC_GetPowerModeState(SMC);
    //asm("wfi");
}

void vApplicationTickHook

(

void

)

Definition at line 77 of file main_freertos_agm02_power_cycling.c.

{
    sfg.queueStatus(&sfg, NORMAL);      // assume NORMAL status for next pass through the loop
}

Here is the call graph for this function:

Variable Documentation

ControlSubsystem controlSubsystem

used for serial communications

Definition at line 65 of file main_freertos_agm02_power_cycling.c.

EventGroupHandle_t event_group = NULL

Definition at line 68 of file main_freertos_agm02_power_cycling.c.

Referenced by fusion_task(), main(), and read_task().

PhysicalSensor sensors[3]

This implementation uses three physical sensors.

Definition at line 67 of file main_freertos_agm02_power_cycling.c.

SensorFusionGlobals sfg

This is the primary sensor fusion data structure.

Definition at line 64 of file main_freertos_agm02_power_cycling.c.

StatusSubsystem statusSubsystem

provides visual (usually LED) status indicator

Definition at line 66 of file main_freertos_agm02_power_cycling.c.